/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
#include "hamming.h"
/**
	hamming.c		Implement Hamming(15,11) code

	Hamming(bitstrm, flag)
	int *bitstrm        data bit stream
	int flag            0 = ENCODE, 1 = DECODE
**/

static void GenParity();
static int Correct();

#define ENCODE 0
#define DECODE 1

/**************************************************************************
*
* ROUTINE
*		Hamming
*
* FUNCTION
*		Implement Hamming (15,11) code
*
* SYNOPSIS
*		subroutine Hamming(*bitstrm, flag)
*
*   formal 
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------	
*	*bitstrm		int	i/o	bitstream
*	flag		int	 i	0 = ENCODE, 1 = DECODE
*
***************************************************************************/


static void GenParity(int *bitstrm, int *loc, int *par)
{ int bits[11];
  register int i;

  for (i = 0; i < 11; i++)
    bits[i] = bitstrm[loc[i]];

  par[0] = bits[10];
  for (i = 0; i <= 6; i += 3)
    par[0] ^= bits[i];

  par[1] = par[0];
  par[0] ^= (bits[1] ^ bits[4] ^ bits[8]);
  par[1] ^= (bits[2] ^ bits[5] ^ bits[9]);

  par[2] = bits[10];
  for (i= 7; i < 10; i++)
    par[2] ^= bits[i];
  
  par[3] = par[2];
  for (i = 1; i <= 3; i++) {
    par[2] ^= bits[i];
    par[3] ^= bits[i+3];
  }
}


static int Correct(int *bitstrm, int *parity, int *ploc)
{ register int i;
  int rpar = 0, gpar = 0, err_loc;
  int correct[16] = {-1, -1, -1, 0, -1, 1, 2, 3, -1, 4, 5, 6, 7, 8, 9, 10};
  int syndrome;

  for (i = 0; i < 4; i++) {
    gpar |= parity[i] << i;
    rpar |= bitstrm[ploc[i]] << i;
  }

  syndrome = gpar ^ rpar;
  err_loc = correct[syndrome];
  
  if (err_loc >= 0)
    bitstrm[err_loc] ^= 0x1;

  return(syndrome);
}

int Hamming(int *bitstrm, int flag)
{ 
  int parity[4], i, retval;
  static int loc[11] = {39, 40, 41, 46, 71, 91, 92, 93, 98, 123, 138};
  static int ploc[4] = {139, 140, 141, 142};

  GenParity(bitstrm, loc, parity);

  if (flag == ENCODE) {
    for (i = 0; i < 4; i++)
      bitstrm[ploc[i]] = parity[i];
    retval = 4;
  }
  else if (flag == DECODE)
    retval = Correct(bitstrm, parity, ploc);
  else
    retval = -1;

  return retval;
}
